import socket
import time
import numpy as np
import json
from flask import Flask, request, jsonify
from time import *
import requests

app = Flask(__name__)

# 打开夹爪
Gripper_release = {"command": "set_gripper_release", "speed": 1000, "force": 1000, "block": True}
# 闭合夹爪
Gripper_pick_on = {"command": "set_gripper_pick_on", "speed": 1000, "force": 1000, "block": True}
# 初始点
move_init_angle = {"command": "movej", "joint": [1957, 117059, -89232, -846, 114787, -7614], "v": 60, "r": 0,
                   "trajectory_connect": 0}
# 拍照前的过渡点
photo_guodudian1 = {"command": "movej", "joint": [-883, 126374, -126046, 1410, -1344, 83932], "v": 60, "r": 0,
                    "trajectory_connect": 0}
# 拍照点
photo_angle = {"command": "movej", "joint": [-89780, 126905, -121607, 1409, -1349, 83912], "v": 60, "r": 0,
               "trajectory_connect": 0}
# 抓取点前方
pick_before = {"command": "movej", "joint": [-90053, 67935, -95728, 1701, 32902, 89707], "v": 60, "r": 0,
               "trajectory_connect": 0}
# 抓取点
pick = {"command": "movel", "pose": [935124, 160459, 1240, -1372, -1547, -1730], "v": 60, "r": 0,
        "trajectory_connect": 0}
# 升降
lift_up = {"command": "set_lift_height", "height": 1500, "speed": 100}
# 放置过渡点
put_guodudian = {"command": "movej", "joint": [-87846, 66772, -103127, 777, -70474, 89925], "v": 60, "r": 0,
                 "trajectory_connect": 0}
# 放置点
put_angle = {"command": "movej", "joint": [-92665, -23597, -127464, 3972, -29615, 82070], "v": 60, "r": 0,
             "trajectory_connect": 0}
# 升降下降
lift_down = {"command": "set_lift_height", "height": 450, "speed": 50}
# 平放放置的过渡点
put_guodudian1 = {"command": "movej", "joint": [-106582, -64528, -113058, 16212, -2083, 62038], "v": 60, "r": 0,
                  "trajectory_connect": 0}
# 平放的放置点
put_final = {"command": "movej", "joint": [-139145, -95548, -73100, 39862, -97329, 2298], "v": 60, "r": 0,
             "trajectory_connect": 0}
# 机械臂切换坐标系
Gripper_tool = { "command": "set_change_tool_frame", "tool_name": "Gripper" }

class ArmController:

    def __init__(self, ip="192.168.10.18", port=8080, camera_service_url="http://localhost:6687/detect"):
        """
        初始化机械臂
        """
        self.robot_ip = ip
        self.robot_port = port
        self.camera_service_url = camera_service_url  # 新增相机服务 URL
        self.socket = None
        self.robot_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.robot_socket.connect((self.robot_ip, self.robot_port))
        print("机械臂初始化成功")
        self.send_arm_command(Gripper_release)
        print("夹爪初始化成功")
        self.send_arm_command(Gripper_tool)
        print("机械臂已切换至Gripper坐标系")
        self.send_arm_command(move_init_angle)
        print("机械臂已到达初始位置待命")

    def send_arm_command(self, command, max_retries=3):
        """
        发送json指令给机械臂动作，并根据device值处理响应，带重试机制
        :param command: json格式的机械臂命令
        :param max_retries: 最大重试次数
        :return: None
        """
        buffer = ""  # 用于累积接收到的数据
        first_message = None  # 存储第一条消息
        target_message = None  # 存储匹配目标device的消息
        retries = 0  # 重试计数

        if not self.robot_socket:
            print("未连接到机械臂")
            return None

        # 将字典转换为JSON字符串
        json_str = json.dumps(command)
        self.robot_socket.sendall((json_str + '\n').encode('utf-8'))
        print(f"已发送机械臂指令: {json_str}")

        while retries < max_retries:
            try:
                while True:
                    try:
                        response = self.robot_socket.recv(1024).decode('utf-8')
                        if not response:
                            print("服务端断开连接")
                            break
                        buffer += response

                        while '\n' in buffer:
                            message, buffer = buffer.split('\n', 1)
                            try:
                                parsed_message = json.loads(message)
                                print(f"Received message: {parsed_message}")

                                # 如果是第一条消息，存储用于state检查
                                if first_message is None:
                                    first_message = parsed_message

                                # 检查device值
                                device_value = parsed_message.get('device')
                                if device_value == 3:
                                    print(f"检测到device=3，跳出循环")
                                    return  # 直接返回

                                # 如果device为0或1，且还未找到目标消息
                                if device_value in (0, 1) and target_message is None:
                                    target_message = parsed_message
                                    state_ok = (first_message.get('state', False) == True or
                                                first_message.get('receive_state', False) == True)
                                    trajectory_state_ok = target_message.get('trajectory_state', False) == True
                                    print(f"State (from first message) OK: {state_ok}, "
                                          f"Trajectory state (from target message) OK: {trajectory_state_ok}")
                                    if state_ok and trajectory_state_ok:
                                        print(f"指令执行完成，device={device_value}，状态验证通过")
                                        return
                                    else:
                                        print("指令执行未完成或状态异常:")
                                        print(f"First message: {first_message}")
                                        print(f"Target message: {target_message}")
                                        raise RuntimeError("机械臂指令执行失败，状态未达到预期")

                            except json.JSONDecodeError:
                                buffer = message + '\n' + buffer
                                break
                    except socket.timeout:
                        print(f"接收响应超时（第 {retries + 1} 次尝试），可能是机械臂未正确响应")
                        if first_message and target_message:
                            state_ok = (first_message.get('state', False) == True or
                                        first_message.get('receive_state', False) == True)
                            trajectory_state_ok = target_message.get('trajectory_state', False) == True
                            if state_ok and trajectory_state_ok:
                                print(f"指令执行完成，超时前已收到有效消息，状态验证通过")
                                return
                        retries += 1
                        if retries < max_retries:
                            print(f"重试 {retries + 1}/{max_retries}")
                            buffer = ""  # 清空缓冲区，重试
                            self.robot_socket.sendall((json_str + '\n').encode('utf-8'))  # 重新发送指令
                            sleep(1)  # 短暂等待后重试
                        break

            except socket.timeout:
                continue  # 外层循环处理重试

        raise RuntimeError(f"机械臂响应超时，经过 {max_retries} 次重试仍未成功")

    def move_to_position(self, height, target_name, state):
        if not target_name:
            return
        # 动作组到达拍照点位
        self.send_arm_command(photo_guodudian1)
        self.send_arm_command(photo_angle)
        if height:
            lift_high = {"command": "set_lift_height", "height": height, "speed": 80}
            # 升降到商品的层高
            self.send_arm_command(lift_high)
            # 已到达拍照点位，开始通过 HTTP 接口调用相机服务
            sleep(1)
            try:
                print(f"target_name =={target_name}")
                response = requests.get(self.camera_service_url, params={"target_name": target_name}, timeout=200)
                response.raise_for_status()
                result = response.json()

                if result["status"] == "success":
                    grasp_pose = result["gripper_pose"]
                    print(grasp_pose["x"])
                    X = float(grasp_pose["x"]) * 1000
                    Y = float(grasp_pose["y"]) * 1000
                    Z = float(grasp_pose["z"]) * 1000
                    print(f"从相机服务获取到的夹爪位置{X},{Y},{Z}")
                else:
                    print("相机服务检测失败:", result.get("message"))
                    return

            except requests.exceptions.RequestException as e:
                print(f"调用相机服务失败: {e}")
                return

            # 抓取高度和拍照高度的偏移量
            offset_height = X * 2.5
            # 上升到抓取时高度
            lift_high_up = {"command": "set_lift_height", "height": height + offset_height, "speed": 100}
            self.send_arm_command(lift_high_up)
            # 移动前后左右偏移
            offset_x = Z * 1000
            offset_y = Y * 1000
            print(offset_y)
            # self.send_arm_command(pick_before)
            move_offset_y = {"command": "movej_p",
                             "pose": [624593 + offset_x - 100000, 220756 - offset_y + 50000, 765, 1574, -1492, 1635],
                             "v": 60, "r": 0, "trajectory_connect": 0}
            # 抓取点前方
            self.send_arm_command(move_offset_y)
            # 抓取点
            move_pick = {"command": "movel",
                         "pose": [624593 + offset_x + 20000, 220756 - offset_y + 50000, 765, 1574, -1492, 1635],
                         "v": 20, "r": 0, "trajectory_connect": 0}
            self.send_arm_command(move_pick)
            self.send_arm_command(Gripper_pick_on)
            # 抓取后上方抬起
            lift_pick_up = {"command": "set_lift_height", "height": height + offset_height + 200, "speed": 100}
            self.send_arm_command(lift_pick_up)
            self.send_arm_command(move_offset_y)
            self.send_arm_command(photo_angle)
            self.send_arm_command(put_guodudian)
            self.send_arm_command(put_angle)
            self.send_arm_command(put_guodudian1)
            self.send_arm_command(put_final)
            self.send_arm_command(lift_down)
            self.send_arm_command(Gripper_release)
        if state == "1":
            self.send_arm_command(lift_up)
            self.send_arm_command(put_guodudian1)
            self.send_arm_command(put_guodudian)
            self.send_arm_command(photo_angle)
            self.send_arm_command(photo_guodudian1)
            self.send_arm_command(move_init_angle)
        else:
            self.send_arm_command(lift_up)
            self.send_arm_command(photo_guodudian1)
            self.send_arm_command(move_init_angle)


arm_controller = ArmController()
count = True


@app.route('/pickProduct', methods=['POST'])
def pickProduct():
    """
    拣选请求
    """
    global target_name, state, count
    try:
        if request.is_json:
            data = request.get_json()
        else:
            data = request.args.to_dict()

        target_name = data.get("target_name")
        height = data.get("arucoHigh")
        state = data.get("state")
        if count:
            arm_controller.send_arm_command(photo_guodudian1)
            arm_controller.send_arm_command(photo_angle)
            count = False
        result = arm_controller.move_to_position(target_name=target_name, height=height, state=state)
        if state != "1":
            count = True
        return jsonify({
            "code": 200,
            "data": True,
            "msg": None
        })
    except Exception as e:
        return jsonify({
            "code": 500,
            "data": False,
            "msg": str(e)
        })


if __name__ == '__main__':
    app.run(host='0.0.0.0', port=6686, debug=False)
